﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.PhysicalModel;
using W3C.Soap;

using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using drive = SimJaguar.QuadDifferentialDrive.Proxy;

namespace SimJaguar.Jaguar
{
    /// <summary>
    /// Jaguar state
    /// </summary>
    [DataContract]
    public class JaguarState
    {
        private Robot jaguar = new Robot();

        [DataMember]
        public Robot Jaguar
        {
            get { return jaguar; }
            set { jaguar = value; }
        }
    }

    [DataContract]
    public class Robot
    {

        private Sensors _Sensors = new Sensors();

        [DataMember]
        public Sensors Sensors
        {
            get { return _Sensors; }
            set { _Sensors = value; }
        }

        private Physics _Physics = new Physics();

        [DataMember]
        public Physics Pose
        {
            get { return _Physics; }
            set { _Physics = value; }
        }

        private QuadDifferentialDrive.Proxy.DriveDifferentialFourWheelState _QuadDrive = new QuadDifferentialDrive.Proxy.DriveDifferentialFourWheelState();

        [DataMember]
        public QuadDifferentialDrive.Proxy.DriveDifferentialFourWheelState Drive
        {
            get { return _QuadDrive; }
            set { _QuadDrive = value; }
        }

        
    }



    [DataContract]
    public class Physics
    {
        [DataMember]
        public double Speed;

        [DataMember]
        public double Heading;

        [DataMember]
        public double X_pose;

        [DataMember]
        public double Y_pose;

        [DataMember]
        public double Z_pose;

        [DataMember]
        public Vector3 PositionVector;

        [DataMember]
        public Vector3 VelocityVector;

        [DataMember]
        public Quaternion OrientationVector;

        private double _LeftFrontSpeed, _LeftRearSpeed, _RightFrontSpeed, _RightRearSpeed;

        [DataMember]
        [Description ("in rads/sec")]
        public double LeftFrontSpeed
        {
            get { return _LeftFrontSpeed; }
            set { _LeftFrontSpeed = value; }
        }

        [DataMember]
        [Description("in rads/sec")]
        public double LeftRearSpeed
        {
            get { return _LeftRearSpeed; }
            set { _LeftRearSpeed = value; }
        }

        [DataMember]
        [Description("in rads/sec")]
        public double RightFrontSpeed
        {
            get { return _RightFrontSpeed; }
            set { _RightFrontSpeed = value; }
        }

        [DataMember]
        [Description("in rads/sec")]
        public double RightRearSpeed
        {
            get { return _RightRearSpeed; }
            set { _RightRearSpeed = value; }
        }

    }


    [DataContract]
    public class Wheel
    {
        public double AxleSpeed;
    }

    [DataContract]
    public class Sensors
    {
        [DataMember]
        public webcam.QueryFrameResponse WebcamData;

        [DataMember]
        public Image WebcamBitmap;

        [DataMember]
        public lrf.State laserData;

    }
}
